#include <thread>
#include <iostream>
#include <fstream>
#include <sys/shm.h>
#include <sys/sem.h>
#include <malloc.h>
#include "zucmodule_cmd.h"
#include "pyport.h"
#include "zucmodule_exceptions.h"
#include "rcs.hh"
#include "rcs_print.hh"
#include "nml_intf/zuc_nml.hh"
#include "timer.hh"
#include "motion/semintf.h"
#include "kinintf.h"
#include "joint_space_smooth.h"
#include "jkutil/iniparser.h"
#include "motion/torque_control_queue.h"

// static PyObject* home(pyCommandChannel* s, PyObject* o)
// {
//     ZUC_JOINT_HOME m;
//     if (!PyArg_ParseTuple(o, "i", &m.joint))
//         return NULL;
//     zucSendCommand(s, m);
//     Py_INCREF(Py_None);
//     return Py_None;
// }

// static PyObject* unhome(pyCommandChannel* s, PyObject* o)
// {
//     ZUC_JOINT_UNHOME m;
//     if (!PyArg_ParseTuple(o, "i", &m.joint))
//         return NULL;
//     zucSendCommand(s, m);
//     Py_INCREF(Py_None);
//     return Py_None;
// }

// static PyObject* override_limits(pyCommandChannel* s, PyObject* o)
// {
//     ZUC_JOINT_OVERRIDE_LIMITS m;
//     m.joint = 0;  // same number for all
//     zucSendCommand(s, m);
//     Py_INCREF(Py_None);
//     return Py_None;
// }